/**
 *             _..--""-.                  .-""--.._
           _.-'         \ __...----...__ /         '-._
         .'      .:::...,'              ',...:::.      '.
        (     .'``'''::;                  ;::'''``'.     )
         \             '-)              (-'             /
          \             /                \             /
           \          .'.-.            .-.'.          /
            \         | \0|            |0/ |         /
            |          \  |   .-==-.   |  /          |
             \          `/`;          ;`\`          /
              '.._      (_ |  .-==-.  | _)      _..'
                  `"`"-`/ `/'        '\` \`-"`"`
                       / /`;   .==.   ;`\ \
                 .---./_/   \  .==.  /   \ \
                / '.    `-.__)       |    `"
               | =(`-.        '==.   ;
                \  '. `-.           /
                 \_:_)   `"--.....-'

                 file: sensor_node.cpp
                 description: sensor node for distance sensor, publish sensor data
                 authors: Michiel and Peter
 */

#include <cmath>
#include <ros/ros.h>
#include <std_msgs/Int32.h>
#include <phidget_ik/phidget_ik.h>
#include "gripper/topics.h"

#define	SENSOR_PORT	0	// port sensor is attached

PhidgetIK phidget_ik;

/**
 * Returns sensor value in cm.
 * @param	port	port to read sensor data
 * @return	sensor	value in cm.
 */
int getSensorValueCm(int port)
{
	int temp = abs(phidget_ik.getSensorValue(SENSOR_PORT) - 11);
	if(temp == 0)
		return 30;	// max value
	return 2076/temp;
}

/**
 * Init sensor, return number of inputs.
 * @return number of inputs.
 */
int init()
{
	phidget_ik.init(-1);
	phidget_ik.waitForAttachment(1000);

	// get number of inputs
	int inputs = phidget_ik.getSensorCount();
	if (phidget_ik.getLastError())
	{
		ROS_ERROR("Error initializing PhidgetInterfaceKit.");
		return 1;
	}

	return inputs;
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "sensor_node");	// node name calc_node
	ros::NodeHandle n;
	init();

	ros::Publisher chatter = n.advertise<std_msgs::Int32>(Topic::IR_DISTANCE, 1000);
	ros::Rate loop_rate(10);

	std_msgs::Int32 msg;

	while(ros::ok())
	{
		msg.data = getSensorValueCm(SENSOR_PORT);

		chatter.publish(msg);	// publish message
		ROS_INFO("Sensor value: %i", msg.data);

		ros::spinOnce();
		loop_rate.sleep();
	}

	return 0;
}
